#pragma once
#ifndef COLLLISION_DETECTOR_H
#define COLLLISION_DETECTOR_H

#include "basic_typedef.h"
#include <CGAL/squared_distance_2.h>
#include <CGAL/minkowski_sum_2.h>
#include <CGAL/Small_side_angle_bisector_decomposition_2.h>

class CollisionDetector
{
public:
    
    ////////////////////////
    // CTORS
    ////////////////////////
    /*	Parameters:
            radius	- radius of robot
            Room	- describes the boundary of the scenario
    */
    CollisionDetector(Polygon_2	robot, Obstacles &obs): 
                m_robot(robot)
    {
        
        // Construction of the polygon set, used for collision detection
        if (!obs.empty())
        {
            Polygon_2 mirrored = robot;//mirror_robot(robot);
            for (Obstacles::iterator iter = obs.begin(); iter != obs.end(); iter++)
            {
                // For every obstacle calculate its Minkowski sum with the "robot"

                // Compute the Minkowski sum using the decomposition approach.
                CGAL::Small_side_angle_bisector_decomposition_2<Kernel>  ssab_decomp;
                Polygon_with_holes_2	poly_wh = minkowski_sum_2(*iter, mirrored, ssab_decomp);

                // Add the result to the polygon set
                m_poly_set.join(poly_wh);
            }
        }
    }

    static Polygon_2 mirror_robot(Polygon_2 robot)
    {
        Polygon_2 result;
        for (int i = 0; i < robot.size(); i++)
        {
            Point_2 poly_point = robot[i];
            Point_2 result_point(- poly_point.x(), - poly_point.y());
            result.push_back(result_point);
        }
        return result;
    }
    
    ~CollisionDetector(){};

    ////////////////////////
    // Queries
    ////////////////////////
    
    /*	Check if the given configuration is collision free (returns true for free samples) */
    bool	valid_conf(Point_2 pos)
    {
        return (m_poly_set.oriented_side(pos) == CGAL::ON_NEGATIVE_SIDE);
    }

    /*	Validate the connection between two configurations by 
        sampling along a line.
        Input:	* Start and target configurations
                * Eps value that determines the size of the steps 
                    that are taken along the line
    */
    bool	local_planner(Point_2 start, Point_2 target, double eps)
    {
        double x1 = start.x().exact().to_double();
        double y1 = start.y().exact().to_double();
        double x2 = target.x().exact().to_double();
        double y2 = target.y().exact().to_double();

        double distance = sqrt(pow(x1 - x2, 2) + pow(y1 - y2,2));

        double step_size = eps;

        // calculate how many steps are required
        int step_num = floor((distance - step_size) / step_size);
        double vx = x2 - x1;
        double vy = y2 - y1;

        for (int i = 1; i <= step_num; i++)
        {
            // generate a configuration for every step
            double offset =  (i * step_size) / (distance - step_size);
            double currx = x1 + vx * offset;
            double curry = y1 + vy * offset;

            Point_2 currentPos(currx, curry);

            // If an intermidiate configuration is invalid, return false
            if (!valid_conf(currentPos))
                return false;
        }

        // GREAT SUCCESS!
        return true;
    }

    ////////////////////////
    // Data Members
    ////////////////////////
    Polygon_2	    m_robot;	// The robot
    Polygon_set_2	m_poly_set; // Polygon set, for collision detection
};

#endif